#ifndef YAW_KALMANFILTER_H
#define YAW_KALMANFILTER_H

#include "postprocess/kalman_filter/base_kalmanfilter.h"
#include "postprocess/common/common_var.h"

// 这个滤波器用于更新yaw
// dynam_param_num=1, int measure_param_num=1,control_param_num = 0
class YawKalmanFilter : public BaseKalmanFilter {
 public:
  YawKalmanFilter();
  YawKalmanFilter(const ProcessorClass &flag);
  YawKalmanFilter(YawKalmanFilter const&) = delete;
  YawKalmanFilter& operator=(YawKalmanFilter const&) = delete;
  YawKalmanFilter(YawKalmanFilter&&) = delete;
  YawKalmanFilter& operator=(YawKalmanFilter&&) = delete;
  ~YawKalmanFilter();

  /// @brief init function
  void Init(int dynam_param_num, int measure_param_num,
            int control_param_num = 0) override;

  /// @brief update filter with object
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  /// @param is_utm_flag
  void UpdateWithObject(TrackedObjectPtr& new_object,
                        TrackedObjectPtr const& old_object,
                        double time_diff) override;

  /// @brief set Q
  /// @param Matrix Q
  void SetQ(Eigen::MatrixXf& Q) override;

  /// @brief set R
  /// @param Matrix R
  void SetR(Eigen::MatrixXf& R) override;

  /// @brief get state of filter
  /// @param yaw
  /// @param
  /// @param
  void GetState(float& yaw) noexcept;


  /// @brief update filter without object
  /// @param time_diff
  void UpdateWithoutObject(double time_diff) override;

  /// @brief set initial position
  /// @param yaw to be set
  /// @return void
  void SetYaw(const float yaw) noexcept;

  /// @brief get position
  /// @param yaw to be get
  /// @return void
  void GetYaw(float& yaw)  noexcept;
  float GetYaw()  noexcept;


  /// @brief get acceleration
  /// @param velocity to be get
  /// @return void
  void GetProcessorClassFlag(ProcessorClass &flag)  noexcept;

  /// @brief get acceleration
  /// @param velocity to be get
  /// @return void
  void SetProcessorClassFlag(ProcessorClass flag) noexcept;


 protected:
  /// @brief kalman filter update state with  observation
  /// @param
  void update_state(TrackedObjectPtr const& new_object,
                    TrackedObjectPtr const& old_object,
                    double time_diff) override;

 private:
  /// @brief yaw angle
  float yaw_;

  /// @brief proseccor flag
  ProcessorClass processor_flag_;
};

#endif
